#include "stdafx.h"
#include "CppUnitTest.h"

#include "HSModel/GeometryUtils.h"

using namespace Microsoft::VisualStudio::CppUnitTestFramework;

namespace HSModelTests
  {
  TEST_CLASS(GeometryUtilsTests)
    {
    public:

      TEST_METHOD(LineBoxShouldIntersect)
        {        
        TVector3 origin = TVector3(0,0,0);
        TVector3 direction = TVector3(1,1,1);
        TLine3 line = TLine3(origin, direction);

        TVector3 center1 = TVector3(5,5,0);
        TVector3 u0 = TVector3(1,0,0);
        TVector3 u1 = TVector3(0,1,0);
        TVector3 u2 = TVector3(0,0,1);
        double ext1 = 3.0;
        double ext2 = 3.0;
        double ext3 = 3.0;
        TBox3 box1 = TBox3(center1, u0, u1, u2, ext1, ext2, ext3);

        TVector3 center2 = TVector3(0,5,5);
        double ext12 = 10.0;
        double ext22 = 2.0;
        double ext32 = 3.0;
        TBox3 box2 = TBox3(center2, u0, u1, u2, ext12, ext22, ext32);

        Assert::IsTrue(IntersectionTest(line, box1));
        Assert::IsTrue(IntersectionTest(line, box2));        
        }

      TEST_METHOD(LineBoxShouldNotIntersect)
        {
        TVector3 center1 = TVector3(5,5,0);
        TVector3 u0 = TVector3(1,0,0);
        TVector3 u1 = TVector3(0,1,0);
        TVector3 u2 = TVector3(0,0,1);
        double ext1 = 3.0;
        double ext2 = 3.0;
        double ext3 = 3.0;
        TBox3 box1 = TBox3(center1, u0, u1, u2, ext1, ext2, ext3);

        TVector3 origin = TVector3(0,0,0);
        TVector3 direction = TVector3(0,1,0);
        TLine3 line = TLine3(origin, direction);

        TVector3 center2 = TVector3(0,5,5);
        double ext12 = 10.0;
        double ext22 = 2.0;
        double ext32 = 3.0;
        TBox3 box2 = TBox3(center2, u0, u1, u2, ext12, ext22, ext32);

        Assert::IsFalse(IntersectionTest(line, box1));
        Assert::IsFalse(IntersectionTest(line, box2));        
        }

      TEST_METHOD(LineTriangleIntersect)
        {
        TVector3 origin = TVector3(0,0,0);
        TVector3 direction = TVector3(1,1,1);
        TLine3 line = TLine3(origin, direction);

        TVector3 v1 = TVector3(1,0,0);
        TVector3 v2 = TVector3(0,1,0);
        TVector3 v3 = TVector3(0,0,1);
        TTriangle3 triangle1 = TTriangle3(v1, v2, v3);

        TVector3 v4 = TVector3(0,0,10);
        TVector3 v5 = TVector3(4,-5,0);
        TVector3 v6 = TVector3(-4,5,0);
        TTriangle3 triangle2 = TTriangle3(v4, v5, v6);

        Assert::IsTrue(IntersectionTest(line, triangle1));
        Assert::IsTrue(IntersectionTest(line, triangle2));

        }

      TEST_METHOD(LineTriangleNotIntersect)
        {
        TVector3 origin = TVector3(0,0,20);
        TVector3 direction = TVector3(1,0,0);
        TLine3 line = TLine3(origin, direction);

        TVector3 v1 = TVector3(1,0,0);
        TVector3 v2 = TVector3(0,1,0);
        TVector3 v3 = TVector3(0,0,1);
        TTriangle3 triangle1 = TTriangle3(v1, v2, v3);

        TVector3 v4 = TVector3(0,0,10);
        TVector3 v5 = TVector3(4,-5,0);
        TVector3 v6 = TVector3(-4,5,0);
        TTriangle3 triangle2 = TTriangle3(v4, v5, v6);

        Assert::IsFalse(IntersectionTest(line, triangle1));
        Assert::IsFalse(IntersectionTest(line, triangle2));
        }

      TEST_METHOD(TriangleBoxIntersect)
        {
        TVector3 center1 = TVector3(0,0,0);
        TVector3 u0 = TVector3(1,0,0);
        TVector3 u1 = TVector3(0,1,0);
        TVector3 u2 = TVector3(0,0,1);
        double ext1 = 7.0;
        double ext2 = 7.0;
        double ext3 = 7.0;
        TBox3 box = TBox3(center1, u0, u1, u2, ext1, ext2, ext3);

        TVector3 v1 = TVector3(1,0,0);
        TVector3 v2 = TVector3(0,1,0);
        TVector3 v3 = TVector3(0,0,1);
        TTriangle3 triangle1 = TTriangle3(v1, v2, v3);

        TVector3 v11 = TVector3(0,0,1);
        TVector3 v21 = TVector3(0,1,0);
        TVector3 v31 = TVector3(1,0,0);
        TTriangle3 triangle2 = TTriangle3(v1, v2, v3);

        Assert::IsTrue(IntersectionTest(triangle1, box));
        Assert::IsTrue(IntersectionTest(triangle2, box));
        }

      TEST_METHOD(TriangleBoxNotIntersect)
        {
        TVector3 center1 = TVector3(0,0,0);
        TVector3 u0 = TVector3(1,0,0);
        TVector3 u1 = TVector3(0,1,0);
        TVector3 u2 = TVector3(0,0,1);
        double ext1 = 7.0;
        double ext2 = 7.0;
        double ext3 = 7.0;
        TBox3 box = TBox3(center1, u0, u1, u2, ext1, ext2, ext3);

        TVector3 v1 = TVector3(100,100,100);
        TVector3 v2 = TVector3(100,100,200);
        TVector3 v3 = TVector3(100,100,10);
        TTriangle3 triangle1 = TTriangle3(v1, v2, v3);

        TVector3 v11 = TVector3(10,200,200);
        TVector3 v21 = TVector3(20,10,10);
        TVector3 v31 = TVector3(10,10,10);
        TTriangle3 triangle2 = TTriangle3(v1, v2, v3);

        Assert::IsFalse(IntersectionTest(triangle1, box));
        Assert::IsFalse(IntersectionTest(triangle2, box));
        }

      TEST_METHOD(BoxBoxIntersect)
        {
        TVector3 center1 = TVector3(5,5,0);
        TVector3 u0 = TVector3(1,0,0);
        TVector3 u1 = TVector3(0,1,0);
        TVector3 u2 = TVector3(0,0,1);
        double ext1 = 7.0;
        double ext2 = 7.0;
        double ext3 = 7.0;
        TBox3 box1 = TBox3(center1, u0, u1, u2, ext1, ext2, ext3);

        TVector3 center2 = TVector3(0,0,0);
        double ext12 = 7.0;
        double ext22 = 7.0;
        double ext32 = 7.0;
        TBox3 box2 = TBox3(center2, u0, u1, u2, ext12, ext22, ext32);

        Assert::IsTrue(IntersectionTest(box1, box2));
        }

      TEST_METHOD(BoxBoxNotIntersect)
        {
        TVector3 center1 = TVector3(5,5,0);
        TVector3 u0 = TVector3(1,0,0);
        TVector3 u1 = TVector3(0,1,0);
        TVector3 u2 = TVector3(0,0,1);
        double ext1 = 7.0;
        double ext2 = 7.0;
        double ext3 = 7.0;
        TBox3 box1 = TBox3(center1, u0, u1, u2, ext1, ext2, ext3);

        TVector3 center2 = TVector3(0,0,100);
        double ext12 = 7.0;
        double ext22 = 7.0;
        double ext32 = 7.0;
        TBox3 box2 = TBox3(center2, u0, u1, u2, ext12, ext22, ext32);

        Assert::IsFalse(IntersectionTest(box1, box2));
        }

      TEST_METHOD(TriangleTriangleIntersect)
        {
        TVector3 v1 = TVector3(1,0,0);
        TVector3 v2 = TVector3(0,1,0);
        TVector3 v3 = TVector3(0,0,1);
        TTriangle3 triangle1 = TTriangle3(v1, v2, v3);

        TVector3 v4 = TVector3(0,0,0);
        TVector3 v5 = TVector3(5,0,5);
        TVector3 v6 = TVector3(0,5,5);
        TTriangle3 triangle2 = TTriangle3(v4, v5, v6);

        Assert::IsTrue(IntersectionTest(triangle1, triangle2));
        }

      TEST_METHOD(TriangleTriangleNotIntersect)
        {
        TVector3 v1 = TVector3(1,0,0);
        TVector3 v2 = TVector3(0,1,0);
        TVector3 v3 = TVector3(0,0,1);
        TTriangle3 triangle1 = TTriangle3(v1, v2, v3);

        TVector3 v4 = TVector3(0,0,10);
        TVector3 v5 = TVector3(4,-5,10);
        TVector3 v6 = TVector3(-4,5,9);
        TTriangle3 triangle2 = TTriangle3(v4, v5, v6);

        Assert::IsFalse(IntersectionTest(triangle1, triangle2));
        }

      TEST_METHOD(RayTriangleIntersect)
        {
        const TVector3 v1 = TVector3(1, 0, 0);
        const TVector3 v2 = TVector3(0, 1, 0);
        const TVector3 v3 = TVector3(0, 0, 1);
        const TTriangle3 triangle = TTriangle3(v1, v2, v3);

        const TVector3 origin = TVector3(0, 0, 0);
        const TVector3 direction = TVector3(1, 1, 1);
        const TRay3 ray = TRay3(origin, direction);

        Assert::IsTrue(IntersectionTest(triangle, ray));
        }

      TEST_METHOD(RayTriangleNotIntersect)
        {
        const TVector3 v1 = TVector3(1, 0, 0);
        const TVector3 v2 = TVector3(0, 1, 0);
        const TVector3 v3 = TVector3(0, 0, 1);
        const TTriangle3 triangle = TTriangle3(v1, v2, v3);

        const TVector3 origin = TVector3(0, 0, 0);
        const TVector3 direction = TVector3(-1, -1, -1);
        const TRay3 ray = TRay3(origin, direction);

        Assert::IsFalse(IntersectionTest(triangle, ray));
        }

      TEST_METHOD(RayTriangleGetIntersectionPointTest)
        {
        const TVector3 v1 = TVector3(1, 0, 0);
        const TVector3 v2 = TVector3(0, 1, 0);
        const TVector3 v3 = TVector3(0, 0, 1);
        const TTriangle3 triangle = TTriangle3(v1, v2, v3);

        const TVector3 origin = TVector3(0, 0, 0);
        const TVector3 direction = TVector3(0, 1, 0);
        const TRay3 ray = TRay3(origin, direction);

        TVector3 point;
        GetIntersectionPoint(point, triangle, ray);

        Assert::AreEqual(0.0, point.X());
        Assert::AreEqual(1.0, point.Y());
        Assert::AreEqual(0.0, point.Z());
        }

      TEST_METHOD(PointToPointDistance)
        {
        const TVector3 v1 = TVector3(1, 0, 2);
        const TVector3 v2 = TVector3(0, 2, 0);

        auto dist = GetDistanceBetweenPoints(v1, v2);

        Assert::AreEqual(3.0, dist);
        }

      TEST_METHOD(PolygonInClockwiseOrder)
        {
        TVector2 a(0, 0), b(0, 5), c (5, 0);
        std::vector<TVector2> points;
        points.push_back(a);
        points.push_back(b);
        points.push_back(c);

        Assert::IsTrue(IsInClockwiseOrder2D(points));
        }

      TEST_METHOD(PolygonInCounterClockwiseOrder)
        {
        TVector2 a(0, 0), b(0, 5), c (5, 0);
        std::vector<TVector2> points;
        points.push_back(c);
        points.push_back(b);
        points.push_back(a);

        Assert::IsFalse(IsInClockwiseOrder2D(points));
        }

      TEST_METHOD(Ray2Box2ShoulFindIntersectionPoint)
        {        
        TVector2 origin (0,0);
        TVector2 direction (1,1);
        TRay2 ray(origin, direction);

        TVector2 center1 (3,0);
        TVector2 u0 (1,0);
        TVector2 u1 (0,1);

        double ext1 = 1.0;
        double ext2 = 1.0;
        TBox2 box1 (center1, u0, u1, ext1, ext2);
        TVector2 result(0,0);

        Assert::IsFalse(GetIntersectionPoint(result,box1,ray) );

        TVector2 center2 (2,0);
        TBox2 box2 (center2, u0, u1, ext1, ext2);

        Assert::IsTrue(GetIntersectionPoint(result,box2,ray) );
        Assert::AreEqual(result[0], 1.0);
        Assert::AreEqual(result[1], 1.0);

        TVector2 center3 (2,1);
        TBox2 box3 (center3, u0, u1, ext1, ext2);
        Assert::IsTrue(GetIntersectionPoint(result,box3,ray) );
        Assert::AreEqual(result[0], 1.0);
        Assert::AreEqual(result[1], 1.0);

        TVector2 center4 (0,0);
        TBox2 box4 (center4, u0, u1, ext1, ext2);
        Assert::IsTrue(GetIntersectionPoint(result,box4,ray) );
        Assert::AreEqual(result[0], 0.0);
        Assert::AreEqual(result[1], 0.0);
        }

      TEST_METHOD(ShoulFindRayProjectionToGround)
        {        
        TVector3 origin_l(10,10,10);
        TVector3 direction_l(1,-1,1);
        TLine3 line(origin_l,direction_l);
        TRay2 ray=GetProjectionOnXY(line);
        TVector2 origin_r=ray.Origin;
        TVector2 direction_r=ray.Direction;

        Assert::AreEqual(10.0, origin_r[0]);
        Assert::AreEqual(10.0, origin_r[1]);
        Assert::AreEqual(1.0, direction_r[0]);
        Assert::AreEqual(-1.0, direction_r[1]);


        direction_l[0]=0;
        direction_l[1]=10;
        direction_l[2]=0;
        TLine3 line1 (origin_l,direction_l);
        ray=GetProjectionOnXY(line1);
        origin_r=ray.Origin;
        direction_r=ray.Direction;

        Assert::AreEqual(10.0, origin_r[0], 10.0);
        Assert::AreEqual(10.0, origin_r[1], 10.0);
        Assert::AreEqual(0.0, direction_r[0]);
        Assert::AreEqual(10.0, direction_r[1]);
        }

      TEST_METHOD(ShouldFindTriangle3ProjectionOnGround)
        {        
        TVector3 first_3D_point(0, 0, 10);
        TVector3 second_3D_point(1, 0, -5);
        TVector3 third_3D_point(0, 1, 12);
        TTriangle3 triangle_3D = TTriangle3(first_3D_point, second_3D_point, third_3D_point);
        auto triangle_2D = GetProjectionOnXY(triangle_3D);
        
        Assert::AreEqual(1.0, triangle_2D.V[1].X());
        Assert::AreEqual(0.0, triangle_2D.V[1].Y());
        }

      TEST_METHOD(ShouldEstimateAreaTriangle2IntersectionCorrectly)
        {        
        TTriangle2 first = TTriangle2(TVector2(1, 1), TVector2(4, 1), TVector2(4, 4));
        TTriangle2 second = TTriangle2(TVector2(1, 1), TVector2(4, 1), TVector2(4, 4));
        bool intersect_on_area;
        intersect_on_area = AreaIntersectionTest (first, second);
        Assert::IsTrue(intersect_on_area);

        second = TTriangle2(TVector2(4, 4), TVector2(7, 4), TVector2(7, 7));
        intersect_on_area = AreaIntersectionTest (first, second);
        Assert::IsFalse(intersect_on_area);

        second = TTriangle2(TVector2(1, 1), TVector2(1, 4), TVector2(4, 4));
        intersect_on_area = AreaIntersectionTest (first, second);
        Assert::IsFalse(intersect_on_area);
        }
    };
  }